#include "my_pid.h"
#include "usart.h"
#include <math.h>

/* X轴控制PID */
PID_IncTypeDef xMotor =
    {
        0.01,   // 比例系数Proportional
        0.001, // 积分系数Integral
        0.00,   // 微分系数Derivative

        0.0,    // 积分项
        1000.0, // 积分项最大值 双边 -imax<i<imax

        0.0,    // 微分项
        1000.0, // 微分项最大值 双边 -dmax<i<dmax

        0.0, // 当前误差
        0.0, // 上一次误差
        0.0, // 上上一次误差

        0.0, // 上一次计算PID的时间

        0.0, // P算法计算后的输出
        0.0, // I算法计算后的输出
        0.0, // D算法计算后的输出

        0.0,  // PID计算后的输出
        30.0, // PID最大输出1000

        MyGetLocPID, // PID计算
        MyReset,     // PID系统复位
};

/* Y轴控制PID */
PID_IncTypeDef yMotor =
    {
        0.01, // 比例系数Proportional
        0.005, // 积分系数Integral
        0.0, // 微分系数Derivative

        0.0,    // 积分项
        1000.0, // 积分项最大值 双边 -imax<i<imax

        0.0,    // 微分项
        1000.0, // 微分项最大值 双边 -dmax<i<dmax

        0.0, // 当前误差
        0.0, // 上一次误差
        0.0, // 上上一次误差

        0.0, // 上一次计算PID的时间

        0.0, // P算法计算后的输出
        0.0, // I算法计算后的输出
        0.0, // D算法计算后的输出

        0.0,    // PID计算后的输出
        1000.0, // PID最大输出1000

        MyGetLocPID, // PID计算
        MyReset,     // PID系统复位
};

// PID控制器计算
// 参数：
//       PID：    需要使用的PID控制器
//       error:   误差=当前值-期望值
// 返回值：
//       output：输出舵机角度 -30~30
int MyGetLocPID(struct PID_IncTypeDef *PID, double error)
{
    PID->error = error; // 记录当前误差

    PID->integrator = PID->integrator + PID->error; // 求出偏差的积分
    if (PID->integrator > PID->imax)
        PID->integrator = PID->imax; // 积分限幅
    else if (PID->integrator < -PID->imax)
        PID->integrator = -PID->imax;

    PID->derivative = PID->error - PID->last_error; // 求出偏差的微分

    PID->OutP = PID->kp * PID->error; // P算法输出

    PID->OutI = PID->ki * PID->integrator; // I算法输出

    PID->OutD = PID->kd * PID->derivative; // D算法输出

    // 位置式PID控制器
    PID->Out = PID->OutP + PID->OutI + PID->OutD;
    if (PID->Out > PID->OutMax)
        PID->Out = PID->OutMax; // 输出限幅
    else if (PID->Out < -PID->OutMax)
        PID->Out = -PID->OutMax;

    PID->last_error = PID->error; // 更新误差值

    return PID->Out; // 输出
}

// PID控制器计算 + 一阶低通RC滤波器
// 参数：
//       PID：    需要使用的PID控制器
//       error:   误差=当前值-期望值
// 返回值：
//       output：输出舵机角度 -30~30
int MyGetRcLocPID(struct PID_IncTypeDef *PID, double error)
{
    PID->error = PID->last_error * 0.4 + error * 0.6; // 记录当前误差 一阶低通RC滤波器

    PID->integrator = PID->integrator + PID->error; // 求出偏差的积分
    if (PID->integrator > PID->imax)
        PID->integrator = PID->imax; // 积分限幅
    else if (PID->integrator < -PID->imax)
        PID->integrator = -PID->imax;

    PID->derivative = PID->error - PID->last_error; // 求出偏差的微分

    PID->OutP = PID->kp * PID->error; // P算法输出

    PID->OutI = PID->ki * PID->integrator; // I算法输出

    PID->OutD = PID->kd * PID->derivative; // D算法输出

    // 位置式PID控制器
    PID->Out = PID->OutP + PID->OutI + PID->OutD;
    if (PID->Out > PID->OutMax)
        PID->Out = PID->OutMax; // 输出限幅
    else if (PID->Out < -PID->OutMax)
        PID->Out = -PID->OutMax;

    PID->last_error = PID->error; // 更新误差值

    return PID->Out; // 输出
}

// 增量式PID控制器计算
// 参数：
//       PID：    需要使用的PID控制器
//       error:   误差=当前值-期望值
// 返回值：
//       output：输出舵机角度 -30~30
int MyIncPID(struct PID_IncTypeDef *PID, double error)
{
    PID->error = error; // 记录当前误差

    PID->derivative = PID->error - PID->last_error; // 求出偏差的微分

    PID->OutP = PID->kp * PID->derivative; // P算法输出

    PID->OutI = PID->ki * PID->error; // I算法输出

    // D算法输出
    PID->OutD = PID->kd * (PID->error - 2 * PID->last_error + PID->last_last_error);

    // 位置式PID控制器
    PID->Out = PID->OutP + PID->OutI + PID->OutD;
    if (PID->Out > PID->OutMax)
        PID->Out = PID->OutMax; // 输出限幅
    else if (PID->Out < -PID->OutMax)
        PID->Out = -PID->OutMax;

    PID->last_error = PID->error; // 更新误差值
    PID->last_last_error = PID->last_error;

    return PID->Out; // 输出
}

// PID控制器初始化
// 参数：
//       PID：    需要初始化的PID控制器
// 返回值：
//       空
void MyReset(struct PID_IncTypeDef *PID)
{
    PID->integrator = 0.0; // 积分项清零
    PID->derivative = 0.0; // 微分项清零
    //    PID->error = 0.0;           	//当前误差清零
    PID->last_error = 0.0;      // 上一次的清零
    PID->last_last_error = 0.0; // 上一次的清零
}

// 设备PID参数
// 参数
//       motor：  0 修改直立PID    1 修改速度PID
//       Kp：     比例常数 放大1000倍
//       Ki：     积分常数 放大1000倍
//       Kd：     微分常数 放大1000倍
// 返回值：
//       空
void SetPid(unsigned char motor, long Kp, long Ki, long Kd)
{
    if (motor == 0)
    {
        xMotor.kp = Kp / 1000.0;
        xMotor.ki = Ki / 1000.0;
        xMotor.kd = Kd / 1000.0;
    }
    else
    {
        yMotor.kp = Kp / 1000.0;
        yMotor.ki = Ki / 1000.0;
        yMotor.kd = Kd / 1000.0;
    }
}

// 显示当前PID的设定情况
// 参数：
//       空
// 返回值：
//       空
void DisplayPid(void)
{
    printf("\r\n");
    printf("UP    %8.4f %8.4f %8.4f\r\n", xMotor.kp, xMotor.ki, xMotor.kd);
    printf("SPEED %8.4f %8.4f %8.4f\r\n", yMotor.kp, yMotor.ki, yMotor.kd);
}

// 显示当前PID的积分
// 参数：
//       空
// 返回值：
//       空
void DisplayPidIntegrator(void)
{
    printf("\r\n");
    printf("xMotor integrator : %lf\r\n", xMotor.integrator);
    printf("yMotor integrator : %lf\r\n", yMotor.integrator);
}
